using System;
using System.Collections.Generic;
using System.Text;

namespace Noein.Math
{
    /// <summary>
    /// 
    /// </summary>
    /// <param name="prevX">previous x</param>
    /// <param name="prevV">derivate of the function at point (t, x)</param>
    /// <param name="h">elapsed time</param>
    /// <returns>current x</returns>
    public delegate float Integrate(float prevX, float prevV, float dt);

    public class NumericalIntegration
    {

        /// <summary>
        /// x(t+dt) = x(t) + dt dx/dt(t)
        /// Simplest numerical integration, but also most unstable and error prone.
        /// </summary>
        public static float Euler(float prevX, float prevV, float dt)
        {
            return prevX + dt * prevV;
        }

        /// <summary>
        /// 2nd order accuracy
        /// </summary>
        public static float Midpoint(float prevX, float prevV, float dt)
        {
            float halfDt = dt * .5f;

            // approximate midpoint between previous x and current x using Euler
            float midX = Euler(prevX, prevV, halfDt);
            // approximate derivate of the function at point (t+dt/2, dx/2)
            float midV = (midX - prevX) / halfDt; 

            return Euler(prevX, midV, dt);
        }

        /// <summary>
        /// 4th order accuracy
        /// </summary>
        public static float RungeKutta4(float prevX, float prevV, float dt)
        {
            float halfDt = dt * .5f;

            float v1 = prevV;                                           // v1 * dt = k1 
            float v2 = (Euler(prevX, v1, halfDt) - prevX) / halfDt;     // v2 * dt = k2
            float v3 = (Euler(prevX, v2, halfDt) - prevX) / halfDt;     // v3 * dt = k3
            float k4 = Euler(prevX, v3, dt) - prevX;     

            // dt * (v1 + 2 * v2 + 2 * v3) + k4 = k1 + 2k2 + 2k3 + k4
            return prevX + (dt * (v1 + 2 * v2 + 2 * v3) + k4) / 6f;
        }
    }
}
